from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch_ros.parameter_descriptions import ParameterValue
from launch.actions import IncludeLaunchDescription,ExecuteProcess

import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    use_sim_time=LaunchConfiguration("use_sim_time",default="false"),

    package_dir = FindPackageShare('robot_xacro_model')
    urdf_path = PathJoinSubstitution([package_dir,'urdf', 'entrance.urdf.xacro'])
    robot_description_content = ParameterValue(Command(['xacro ', urdf_path]), value_type=str)

    empty_gazebo_launch_py = IncludeLaunchDescription(
        PathJoinSubstitution([FindPackageShare("robot_xacro_model"),"launch","gazebo","empty_gazebo.launch.py"]),
    )
    import_urdf2gazebo_launch_py = IncludeLaunchDescription(
        PathJoinSubstitution([FindPackageShare("robot_xacro_model"),"launch","gazebo","import_urdf2gazebo.launch.py"]),
    )

    rviz2_node = Node(
        package="rviz2",
        executable="rviz2",
        arguments=['-d', os.path.join(get_package_share_directory("robot_xacro_model"), 'rviz', 'rviz2_config.rviz')]
    )

    joint_state_publisher_gui = Node(
        package="joint_state_publisher_gui",
        executable="joint_state_publisher_gui",
        name='joint_state_publisher_gui',
        # arguments=[urdf_path]
    )
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"use_sim_time":use_sim_time,'robot_description': robot_description_content}],
        # arguments=[urdf]
    )

    load_joint_state_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
             'joint_state_broadcaster'],
        output='screen'
    )

    load_dd_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
             'diff_drive_base_controller'],
        output='screen'
    )

    return LaunchDescription([
        # rviz2_node,
        empty_gazebo_launch_py,
        import_urdf2gazebo_launch_py,
        joint_state_publisher_gui,
        robot_state_publisher,
        # load_joint_state_controller,
        load_dd_controller,
    ])